#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <iostream>
#include <thread>
#include <mutex>

class KeyServoPublisher : public rclcpp::Node
{
public:
  KeyServoPublisher() : Node("key_servo_publisher")
  {
    pub_ = create_publisher<geometry_msgs::msg::TwistStamped>(
        "/force_servo_control_node/delta_twist_cmds", 10);

    // 定时发布，周期 100ms
    timer_ = create_wall_timer(
        std::chrono::milliseconds(60),
        std::bind(&KeyServoPublisher::timer_callback, this));

    // 开一个线程监听键盘输入
    input_thread_ = std::thread(std::bind(&KeyServoPublisher::keyboardLoop, this));
  }

  ~KeyServoPublisher()
  {
    running_ = false;
    if (input_thread_.joinable())
      input_thread_.join();
  }

private:
  void keyboardLoop()
  {
    while (rclcpp::ok() && running_)
    {
      std::cout << "输入 w(前进) / s(后退) / q(退出): ";
      std::string input;
      std::getline(std::cin, input);

      std::lock_guard<std::mutex> lock(mtx_);
      if (input == "q" || input == "Q")
      {
        RCLCPP_INFO(get_logger(), "退出键盘输入循环");
        rclcpp::shutdown();
        return;
      }
      else if (input == "w" || input == "W")
      {
        vx_ = 0.03;
        RCLCPP_INFO(get_logger(), "持续前进 vx=%.2f", vx_);
      }
      else if (input == "s" || input == "S")
      {
        vx_ = -0.03;
        RCLCPP_INFO(get_logger(), "持续后退 vx=%.2f", vx_);
      }
      else
      {
        vx_ = 0.0;  // 输入其他键则停下
        RCLCPP_INFO(get_logger(), "停止 vx=%.2f", vx_);
      }
    }
  }

  void timer_callback()
  {
    std::lock_guard<std::mutex> lock(mtx_);
    geometry_msgs::msg::TwistStamped cmd;
    cmd.header.stamp = now();
    cmd.header.frame_id = "base_link";
    cmd.twist.linear.x = vx_;

    pub_->publish(cmd);
  }

  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::thread input_thread_;
  bool running_{true};

  std::mutex mtx_;
  double vx_{0.0};  // 当前速度
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<KeyServoPublisher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
